/* Copyright (c) 2022 Qualcomm Technologies, Inc.
 * All Rights Reserved.
 * Confidential and Proprietary - Qualcomm Technologies, Inc.
 */

#ifndef _CAMERA_D435I_H_
#define _CAMERA_D435I_H_

#include <librealsense2/rs.hpp>
#include <mutex>
#include <stdlib.h>
#include <opencv2/opencv.hpp>

#define IMAGE_WIDTH  1280
#define IMAGE_HEIGHT 720


struct CameraIntrinsic
{
    int32_t inputPixelWidth;
    int32_t inputPixelHeight;
    int32_t outputPixelWidth;
    int32_t outputPixelHeight;

    float inputCameraMatrix[9];
    float distortionCoefficient[12];
    float outputCameraMatrix[9];
};

class Camera_D435i
{
public:
    rs2::frameset frames;
    Camera_D435i();
    ~Camera_D435i();
    Camera_D435i(const Camera_D435i & ) = delete;

    bool start();
    bool stop();
    void getImage(cv::Mat & m, uint16_t* depth);

    void frameLock();
    void frameUnlock();

    cv::Point2f pixel2CamNorm(const cv::Point2f & p);
    cv::Point3f pixel2Cam(const cv::Point2f & p, float depth);
    float calcTheta(int box_center_x);

    const CameraIntrinsic & getCameraConfiguration( ) const;

private:
    /* time stamp */
    int64_t realClock;
    int64_t monotonicClock;
    int64_t clockOffset;

    std::mutex frameMutex;
    int32_t imwriteNum;
    char imwritePath[50];

    float h_fov;

    rs2::pipeline pipe;
    rs2::pipeline_profile profiles;
    rs2::config cfg;
    rs2_intrinsics intrinsics;
    CameraIntrinsic configuration;
};
#endif